#include "motion_estimation.h"
namespace perception
{
    namespace motion_estimation
    {
        MotionEstimation::MotionEstimation(/* args */)
        {
        }

        MotionEstimation::~MotionEstimation()
        {
        }
        void MotionEstimation::Process(XYZI_PTR &data_in, XYZI_PTR &data_out, Localization &loc, VehicleStatus &vs, double &lidar_time)
        {
            // 1.匹配定位与车辆状态两个msg。因为两个不同，所以需要对齐时间戳
            if (MatchTime(loc, vs) == false)
            {
                data_out = data_in;
                return;
            }

            // 2.速度低于5km/h时，不进行这个
            if (vs.speed < 5.0 / 3.6)
            {
                data_out = data_in;
                return;
            }
            // 3.旋转变量
            for (auto p : data_in->points)
            {
                float yaw_delta = GetYawDelta(loc);
                float speed_x = vs.speed * std::cos(yaw_delta / 180.0 * M_PI);
                float speed_y = vs.speed * std::sin(yaw_delta / 180.0 * M_PI);
                XYZI_1 point;
                auto now_time = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
                double time = (now_time.count() / 1000.0 - lidar_time);
                point.x = p.x - time * speed_x;
                point.y = p.y - time * speed_y;
                point.z = p.z;
                point.intensity = p.intensity;
                data_out->points.emplace_back(point);
            }
            old_yaw = loc.angle;
        }
        bool MotionEstimation::MatchTime(Localization &loc, VehicleStatus &vs)
        {
            if (std::fabs(loc.time - vs.time) * 1000 > 50 || vs.time < 1 || loc.time < 1)
                return false;
            else
                return true;
        }
        float MotionEstimation::GetYawDelta(Localization &loc)
        {
            float yaw_delta = loc.angle - old_yaw;
            if (yaw_delta > 300)
                yaw_delta = yaw_delta - 360;
            else if (yaw_delta < -300)
                yaw_delta = yaw_delta + 360;
            return -yaw_delta;
        }
    }
}